﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary.Transformation
{
    /// <summary>
    /// This class provides methods to interpolate the seven parameters for a helmert transformation.
    /// </summary>
    [Serializable]
    public class HelmertTransformationInterpolation
    {
        /// <summary>
        /// The first reference point in system A.
        /// </summary>
        private Point3D firstPointSystemA;

        /// <summary>
        /// The second reference point in system A.
        /// </summary>
        private Point3D secondPointSystemA;

        /// <summary>
        /// The third reference point in system A.
        /// </summary>
        private Point3D thirdPointSystemA;

        /// <summary>
        /// The first reference point in system B.
        /// </summary>
        private Point3D firstPointSystemB;

        /// <summary>
        /// The second reference point in system B.
        /// </summary>
        private Point3D secondPointSystemB;

        /// <summary>
        /// The third reference point in system B.
        /// </summary>
        private Point3D thirdPointSystemB;

        /// <summary>
        /// Initializes a new instance of the <see cref="HelmertTransformationInterpolation"/> class.
        /// </summary>
        public HelmertTransformationInterpolation()
        {
            this.firstPointSystemA = new Point3D();
            this.secondPointSystemA = new Point3D();
            this.thirdPointSystemA = new Point3D();
            this.firstPointSystemB = new Point3D();
            this.secondPointSystemB = new Point3D();
            this.thirdPointSystemB = new Point3D();
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="HelmertTransformationInterpolation"/> class.
        /// </summary>
        /// <param name="firstPointSystemA">The first reference point for system A.</param>
        /// <param name="secondPointSystemA">The second reference point for system A.</param>
        /// <param name="thirdPointSystemA">The third reference point for system A.</param>
        /// <param name="firstPointSystemB">The first reference point for system B.</param>
        /// <param name="secondPointSystemB">The second reference point for system B.</param>
        /// <param name="thirdPointSystemB">The third reference point for system B.</param>
        public HelmertTransformationInterpolation(Point3D firstPointSystemA, Point3D secondPointSystemA,
                                                  Point3D thirdPointSystemA, Point3D firstPointSystemB,
                                                  Point3D secondPointSystemB,
                                                  Point3D thirdPointSystemB)
        {
            if (firstPointSystemA == (Point3D) null)
            {
                throw new ArgumentNullException("firstPointSystemA");
            }
            if (secondPointSystemA == (Point3D) null)
            {
                throw new ArgumentNullException("secondPointSystemA");
            }
            if (thirdPointSystemA == (Point3D) null)
            {
                throw new ArgumentNullException("thirdPointSystemA");
            }

            if (firstPointSystemB == (Point3D) null)
            {
                throw new ArgumentNullException("firstPointSystemB");
            }
            if (secondPointSystemB == (Point3D) null)
            {
                throw new ArgumentNullException("secondPointSystemB");
            }
            if (thirdPointSystemB == (Point3D) null)
            {
                throw new ArgumentNullException("thirdPointSystemB");
            }

            this.firstPointSystemA = firstPointSystemA.Copy();
            this.secondPointSystemA = secondPointSystemA.Copy();
            this.thirdPointSystemA = thirdPointSystemA.Copy();
            this.firstPointSystemB = firstPointSystemB.Copy();
            this.secondPointSystemB = secondPointSystemB.Copy();
            this.thirdPointSystemB = thirdPointSystemB.Copy();
        }

        /// <summary>
        /// Gets or sets the first reference point for system A.
        /// </summary>
        /// <value>The first reference point for system A.</value>
        public Point3D FirstPointSystemA
        {
            get { return this.firstPointSystemA; }
            set { this.firstPointSystemA = value; }
        }

        /// <summary>
        /// Gets or sets the second reference point for system A.
        /// </summary>
        /// <value>The second reference point for system A.</value>
        public Point3D SecondPointSystemA
        {
            get { return this.secondPointSystemA; }
            set { this.secondPointSystemA = value; }
        }

        /// <summary>
        /// Gets or sets the third reference point for system A.
        /// </summary>
        /// <value>The third reference point for system A.</value>
        public Point3D ThirdPointSystemA
        {
            get { return this.thirdPointSystemA; }
            set { this.thirdPointSystemA = value; }
        }

        /// <summary>
        /// Gets or sets the first reference point for system B.
        /// </summary>
        /// <value>The first reference point for system B.</value>
        public Point3D FirstPointSystemB
        {
            get { return this.firstPointSystemB; }
            set { this.firstPointSystemB = value; }
        }

        /// <summary>
        /// Gets or sets the second reference point for system B.
        /// </summary>
        /// <value>The second reference point for system B.</value>
        public Point3D SecondPointSystemB
        {
            get { return this.secondPointSystemB; }
            set { this.secondPointSystemB = value; }
        }

        /// <summary>
        /// Gets or sets the third reference point for system B.
        /// </summary>
        /// <value>The third reference point for system B.</value>
        public Point3D ThirdPointSystemB
        {
            get { return this.thirdPointSystemB; }
            set { this.thirdPointSystemB = value; }
        }

        /// <summary>
        /// Interpolates the transformation parameters for a six parameter transformation.
        /// </summary>
        /// <returns>The transformation parameters for a six parameter transformation.</returns>
        public HelmertTransformationParameters InterpolateTransformationParameters()
        {
            Double[,] matrixValues = new Double[9,11];

            matrixValues[0, 0] = 1;
            matrixValues[0, 1] = 0;
            matrixValues[0, 2] = 0;
            matrixValues[0, 3] = 0;
            matrixValues[0, 4] = this.firstPointSystemA.Z;
            matrixValues[0, 5] = -this.firstPointSystemA.Y;
            matrixValues[0, 6] = this.firstPointSystemA.X;
            matrixValues[0, 7] = 0;
            matrixValues[0, 8] = 0;
            matrixValues[0, 9] = 0;
            matrixValues[0, 10] = this.firstPointSystemB.X;

            matrixValues[1, 0] = 0;
            matrixValues[1, 1] = 1;
            matrixValues[1, 2] = 0;
            matrixValues[1, 3] = -this.firstPointSystemA.Z;
            matrixValues[1, 4] = 0;
            matrixValues[1, 5] = this.firstPointSystemA.X;
            matrixValues[1, 6] = this.firstPointSystemA.Y;
            matrixValues[1, 7] = 0;
            matrixValues[1, 8] = 0;
            matrixValues[1, 9] = 0;
            matrixValues[1, 10] = this.firstPointSystemB.Y;

            matrixValues[2, 0] = 0;
            matrixValues[2, 1] = 0;
            matrixValues[2, 2] = 1;
            matrixValues[2, 3] = this.firstPointSystemA.Y;
            matrixValues[2, 4] = -this.firstPointSystemA.X;
            matrixValues[2, 5] = 0;
            matrixValues[2, 6] = this.firstPointSystemA.Z;
            matrixValues[2, 7] = 0;
            matrixValues[2, 8] = 0;
            matrixValues[2, 9] = 0;
            matrixValues[2, 10] = this.firstPointSystemB.Z;

            matrixValues[3, 0] = 1;
            matrixValues[3, 1] = 0;
            matrixValues[3, 2] = 0;
            matrixValues[3, 3] = 0;
            matrixValues[3, 4] = this.secondPointSystemA.Z;
            matrixValues[3, 5] = -this.secondPointSystemA.Y;
            matrixValues[3, 6] = this.secondPointSystemA.X;
            matrixValues[3, 7] = 0;
            matrixValues[3, 8] = 0;
            matrixValues[3, 9] = 0;
            matrixValues[3, 10] = this.secondPointSystemB.X;

            matrixValues[4, 0] = 0;
            matrixValues[4, 1] = 1;
            matrixValues[4, 2] = 0;
            matrixValues[4, 3] = -this.secondPointSystemA.Z;
            matrixValues[4, 4] = 0;
            matrixValues[4, 5] = this.secondPointSystemA.X;
            matrixValues[4, 6] = this.secondPointSystemA.Y;
            matrixValues[4, 7] = 0;
            matrixValues[4, 8] = 0;
            matrixValues[4, 9] = 0;
            matrixValues[4, 10] = this.secondPointSystemB.Y;

            matrixValues[5, 0] = 0;
            matrixValues[5, 1] = 0;
            matrixValues[5, 2] = 1;
            matrixValues[5, 3] = this.secondPointSystemA.Y;
            matrixValues[5, 4] = -this.secondPointSystemA.X;
            matrixValues[5, 5] = 0;
            matrixValues[5, 6] = this.secondPointSystemA.Z;
            matrixValues[5, 7] = 0;
            matrixValues[5, 8] = 0;
            matrixValues[5, 9] = 0;
            matrixValues[5, 10] = this.secondPointSystemB.Z;

            matrixValues[6, 0] = 1;
            matrixValues[6, 1] = 0;
            matrixValues[6, 2] = 0;
            matrixValues[6, 3] = 0;
            matrixValues[6, 4] = this.thirdPointSystemA.Z;
            matrixValues[6, 5] = -this.thirdPointSystemA.Y;
            matrixValues[6, 6] = this.thirdPointSystemA.X;
            matrixValues[6, 7] = 0;
            matrixValues[6, 8] = 0;
            matrixValues[6, 9] = 0;
            matrixValues[6, 10] = this.thirdPointSystemB.X;

            matrixValues[7, 0] = 0;
            matrixValues[7, 1] = 1;
            matrixValues[7, 2] = 0;
            matrixValues[7, 3] = -this.thirdPointSystemA.Z;
            matrixValues[7, 4] = 0;
            matrixValues[7, 5] = this.thirdPointSystemA.X;
            matrixValues[7, 6] = this.thirdPointSystemA.Y;
            matrixValues[7, 7] = 0;
            matrixValues[7, 8] = 0;
            matrixValues[7, 9] = 0;
            matrixValues[7, 10] = this.thirdPointSystemB.Y;

            matrixValues[8, 0] = 0;
            matrixValues[8, 1] = 0;
            matrixValues[8, 2] = 1;
            matrixValues[8, 3] = this.thirdPointSystemA.Y;
            matrixValues[8, 4] = -this.thirdPointSystemA.X;
            matrixValues[8, 5] = 0;
            matrixValues[8, 6] = this.thirdPointSystemA.Z;
            matrixValues[8, 7] = 0;
            matrixValues[8, 8] = 0;
            matrixValues[8, 9] = 0;
            matrixValues[8, 10] = this.thirdPointSystemB.Z;

            Matrix solveMatrix = new Matrix(matrixValues);
            HelmertTransformationParameters parameters = new HelmertTransformationParameters();
            Double[] results = solveMatrix.SolveLinearSystem();

            parameters.TranslationX = results[0];
            parameters.TranslationY = results[1];
            parameters.TranslationZ = results[2];
            parameters.RotationX = results[3];
            parameters.RotationY = results[4];
            parameters.RotationZ = results[5];
            parameters.Scale = results[6] - 1; // 1 + ExtendedMath.Fabs(results[6]);

            //if (results[6] < 0) 
            //{
            //    parameters.Scale = -parameters.Scale;
            //}

            return parameters;
        }
    }
}